﻿using System;
using System.Collections.Generic;
using System.Linq;
using System.Text;
using Microsoft.Xna.Framework;

namespace Oops.Xna.Framework.Physics
{
	/// <summary>
	/// Defines a simplified velocity-matching constraint.
	/// </summary>
	public class VelocityConstraint : Constraint
	{
		private RigidBody rigidBody;

		public Vector3 TargetVelocity;

		public float TimeScale = 0.2f;

		/// <summary>
		/// Gets or sets the <see cref="RigidBody"/> involved in the <see cref="VelocityConstraint"/>.
		/// </summary>
		public RigidBody RigidBody
		{
			get { return this.rigidBody; }
			set
			{
				// Make sure rigid body-to-constaint associations are still valid.
				if ((this.Flags & ConstraintFlags.Registered) == ConstraintFlags.Registered)
				{
					if (this.rigidBody != null)
					{
						this.rigidBody.Constraints.Items.Remove(this);
					}
					if (value != null)
					{
						value.Constraints.Items.Add(this);
					}
				}

				// Hold onto value.
				this.rigidBody = value;
			}
		}

		#region Constraint Members

		internal protected override void OnAdded()
		{
			// Call inherited method.
			base.OnAdded();

			// Associate the constraint with the rigid body.
			if (this.rigidBody != null)
			{
				this.rigidBody.Constraints.Items.Add(this);
			}
		}

		internal protected override void OnRemoved()
		{
			// Call inherited method.
			base.OnRemoved();

			// Disassociated the constraint with the rigid body.
			if (this.rigidBody != null)
			{
				this.rigidBody.Constraints.Items.Remove(this);
			}
		}

		internal protected override void PreStep(float elapsed)
		{
			// Reset the satisfied flag.
			this.satisfied = false;
		}

		internal protected override bool Solve(float elapsed)
		{
			// Assume the constraint has not been broken.
			this.satisfied = true;

			// Exit if the rigid body is disabled.
			if ((this.rigidBody.Flags & RigidBodyFlags.Disabled) == RigidBodyFlags.Disabled)
			{
				return false;
			}

#if INLINE
			// Get the velocity difference.
			float temp = elapsed * this.TimeScale;
			float x = (this.TargetVelocity.X - this.rigidBody.Velocity.X) * temp;
			float y = (this.TargetVelocity.Y - this.rigidBody.Velocity.Y) * temp;
			float z = (this.TargetVelocity.Z - this.rigidBody.Velocity.Z) * temp;

			// Exit if no correction is necessary. 
			if (x * x + y * y + z * z < MathExtension.Epsilon * MathExtension.Epsilon)
			{
				return false;
			}

			// Apply the correction.
			this.rigidBody.Velocity.X += x;
			this.rigidBody.Velocity.Y += y;
			this.rigidBody.Velocity.Z += z;
#elif REFERENCE
			// Get the velocity difference.
			Vector3 velocity;
			Vector3.Subtract(ref this.TargetVelocity, ref this.rigidBody.Velocity, out velocity);
			Vector3.Multiply(ref velocity, elapsed * this.TimeScale, out velocity);
				
			// Exit if no correction is necessary.
			if (Vector3Extension.IsZeroLength(ref velocity))
			{
				return false;
			}

			// Apply the correction.
			Vector3.Add(ref this.rigidBody.Velocity, ref velocity, out this.rigidBody.Velocity);
#else
			// Get the velocity difference.
			Vector3 velocity = (this.TargetVelocity - this.rigidBody.Velocity) * elapsed * this.TimeScale;

			// Exit if no correction is necessary.
			if (velocity.IsZeroLength())
			{
				return false;
			}

			// Apply the correction.
			this.rigidBody.Velocity += velocity;
#endif		
			// Reset all collisions and constraints - a velocity change occurred.
			this.rigidBody.CollisionGroup.Collisions.Reset();
			this.rigidBody.Constraints.Reset();

			// Keep this constraint satisfied.
			//this.satisfied = true;

			// Some more processing may be necessary.
			return true;
		}

		#endregion
	}
}
